Unified navigation and inertial target tracking estimation system

ABSTRACT

A target tracking method uses sensor(s) producing target signals subject to positional and/or angular bias, which are updated with sensor bias estimates to produce updated target-representative signals. Time propagation produces time-updated target states and sensor positional and angular biases. The Jacobian of the state dynamics of a target model produces the state transition matrix for extended Kalman filtering. Target state vector and bias covariances of the sensor are time propagated. The Kalman measurement residual is computed to produce state corrections, which are added to the time updated filter states to thereby produce (i) target state updates and (ii) sensor positional and angular bias updates. The covariance of a state vector comprising target states and sensor positional and angular biases is propagated, producing measurement updated state covariance including (i) target position and velocity measurement covariance updates and (ii) the sensor positional and angular bias measurement covariance updates.

This invention was made with Government support under ContractN00024-03-C-6110 awarded by the Department of the Navy. The Governmenthas certain rights in this invention.

FIELD OF THE INVENTION

This invention relates to the tracking of targets by the use of sensorswhich may have positional and or angular bias or misalignment, andprocessing of information to produce unbiased estimates of targetstates.

BACKGROUND OF THE INVENTION

Collaborative sensor coordination among the systems of a System ofSystems (SOS) is currently being pursued by the Missile Defense Agency(MDA) to enhance both targeting and cueing accuracies in support ofballistic missile countermeasures or defense. The Navy is deploying itsCooperative Engagement Coordination (CEC) system which is intended toenable Aegis destroyers to pass tactical data among or between elementsof the battle group. The MDA will require sensor coordination in orderto provide effective and layered tactical and strategic missile defensein a missile defense System of Systems (SOS). Collaborative sensorcoordination requires each element of a Missile Defense System toregister its sensor(s) to local geodetic coordinate systems in order tominimize tracking and guidance errors, thereby reducing system handoverand guidance errors between the target tracking and/or cueing systemsand the interceptor(s). This “sensor registration” ultimately providesadditional margin to the weapon system's pointing and divert errorbudgets, which in turn expands the battle space and enhances the overallwarfare capability.

FIG. 1 illustrates a scenario 10 in which a first ship 1 and a secondship 2 lie at distances from a land mass 3. Item 6 represents thehorizon. A communication satellite 4 is illustrated, and can communicatewith both ships 1 and 2 by way of paths illustrated as “lightning bolts”4 a and 4 b. Positional calibration measurements for ships 1 and 2 canbe provided by global positioning system (GPS) signals 5 s flowing fromGPS satellites such as 5 a and 5 b, which follow various orbital paths,illustrated together as 5 t. In the scenario 10 of FIG. 1, a hostilemissile or target 12 is launched from a location 12 s on land mass 3,and follows a path or trajectory 12 t. In this scenario, defensive ship2 is located closer to the missile launch site 12 s than ship 1, and itacquires a sensor track earlier than ship 1. In this case, the sensortarget track may use information from radar or infrared sensors. Thetarget track information generated by ship 2 may be communicated to ship1 by way of communications satellite 4, or it may be communicated by adirect path illustrated as 14. The sensors aboard ship 1 can fuse thedata provided from the sensors aboard ship 1 with data from ship 2 toaid in acquiring its own track of the target missile 12. Ship 1 can thenproceed to fire a weapon at the target missile 12. In the scenario 10 ofFIG. 1, the weapon is an antimissile vehicle 16. Antimissile vehicle 16follows a track 16 t to intercept the target missile 12 at an interceptlocation 18.

Current technology in multisensor data fusion assumes that sensor andsystem bias registration techniques can be either (a) accounted forthrough covariance inflation techniques or (b) mitigated through use of‘buffer states.’ The Cooperative Engagement Capability (CEC) Systemdeveloped by Johns Hopkins University/Applied Physics Lab (JHU/APL), isan example of the covariance inflation technique. An example of thebuffer state mitigation technique is described in U.S. Pat. No.7,026,980 entitled MISSILE IDENTIFICATION AND TRACKING SYSTEM AND METHOD(MDOTS) and issued Apr. 11, 2006 in the name of Mavroudakis et al., Thismethod uses the Unified Unbiased Rocket Equation Extended KalmanAlgorithm (UUREEKA) described in U.S. patent application Ser. No.10/972,943 entitled Computerized Method for Generating Low-BiasEstimates of Position of a Vehicle From Sensor Data, filed on Oct. 25,2004 in the name of Boka et al. These techniques may under certaincircumstances result in less-than-optimal fused track statesattributable to sensor registration bias error.

The current art in sensor bias registration methods can be categorizedinto either real-time and non-real-time, and can alternatively becategorized as angular bias methods and positional bias methods. ARCHER,developed by Computer Science Corporation (CSC) and System CalibrationUsing Satellites (SCUS), developed by Lockheed Martin (LMCO), areexamples of non-real-time method for angular registration biasestimation. Both of these methods make use of data in the form ofsatellite ephemeris to provide a reference which is used to estimate theangular bias error. SCUS and Instantaneous Sensor AlignmentAuto-Calibration (ISAAC) described in U.S. patent application Ser. No.11/149,692, filed Jun. 10, 2005 in the name of Boka et al are examplesof angular bias registration methods. Sensor positional biasregistration error amelioration or correction is described in U.S.patent application Ser. No. 11/504,561 and entitled “Method forCompensating for the Positional Errors of a Sensor,” (GPSLess) filed onor about Aug. 14, 2006 in the name of Mookerjee et al.

Improved or alternative sensor registration techniques and or methodsare desired.

SUMMARY OF THE INVENTION

A method according to an aspect of the invention is for target tracking.The method comprises the step of sensing a target to be tracked with atleast one sensor, which sensor or sensors is/are subject to positionalbias and angular bias. The sensor(s) produce target representativesignals subject to sensor positional and angular biases. The sensorpositional bias and the sensor angular bias are updated with any sensorpositional bias updates and sensor angular bias updates, respectively,to thereby produce sensor positional-bias and angular-bias updatedtarget representative signals. State time propagation is performed,including propagation of at least the sensor positional and angular biasand the target positional and velocity states of the sensorpositional-bias and angular-bias updated target representative signalsto produce time updated target states and sensor positional and angularbiases. The Jacobian is computed of the state dynamics of a target modelto produce the state transition matrix for extended Kalman filtering.Covariance time propagation is performed by time propagating thecovariance of a state vector comprising at least position and velocityof the target and positional and angular bias of the sensor, to therebyproduce time updated state covariance. A determination is made as towhether a sensor measurement update is available. If the sensormeasurement update is not available, the steps of time propagating thestate estimates, computing the Jacobian, and time propagating the statecovariance matrix are repeated. If the sensor measurement update isavailable, the Kalman filter gain is determined and the Kalmanmeasurement residual is computed. The Kalman measurement residual isweighted with the gain to produce state corrections, and the statecorrections are added to the time updated target states and sensorpositional and angular biases, to thereby produce updates of

(i) target position and velocity state estimates, and

(ii) sensor positional bias updates and sensor angular bias stateestimates.

Using the Kalman filter gain, updates are made to the state covariancematrix which consists of

(i) target position and velocity measurement covariance updates, and

(ii) sensor positional bias measurement covariance updates and sensorangular bias measurement covariance updates.

In an advantageous mode, the method further comprises the step oftransformation of coordinates of the state vector and state covarianceof the Kalman filter. The transformation of coordinates may be betweenthe sensor bias filter frame and an unbiased filter frame.

A method according to another aspect of the invention is forcompensating for the positional and angular alignment errors of a sensortracking a target with known acceleration. This aspect of the methodcomprises the step of defining the estimator state given by

$\underset{\_}{s} = \begin{Bmatrix}{\underset{\_}{X}}_{\overset{\sim}{E}} \\{\underset{\_}{\overset{.}{X}}}_{\overset{\sim}{E}} \\{\delta{\underset{\_}{R}}_{\overset{\sim}{E}}} \\{\delta\underset{\_}{\theta}}\end{Bmatrix}$and comprising the target position X_({tilde over (E)}), target velocity{dot over (X)}_({tilde over (E)}), and the positional bias δR_({tilde over (E)}) and angular bias δθ of the sensor. This aspect ofthe method also comprises the step of applying to the sensed informationany sensor positional bias update information and angular biasinformation, to produce updated sensed information. This updated sensedinformation ultimately provides improved target state information. Stateestimates are propagated from the previous time to the current time.From the Jacobian, the state transition matrix is computed for theextended Kalman filter algorithm. Using the state transition matrix, thecovariance of a state vector is time propagated, comprising the positionand velocity states of the target and the positional bias of the sensor

In this advantageous mode, the step of propagating the state of theupdated sensed information to produce time updated state estimates ofthe target position may further comprise the step of calculating anonlinear propagation equation making use of the Jacobian matrix. Alsoin this advantageous mode, the step of propagating the state of theupdated sensed information to produce time updated state estimates ofthe target position may further comprise the step of generating aJacobian matrix which provides observability of the sensor positionalbias or error and angular bias (registration error) through gravitationand coriolis forces. Further, the step of time propagating thecovariance of the state vector of the target may comprise the step oftime propagating a composite state vector, where the composite statevector comprises the position and velocity states of the target and theposition bias and angular bias of the sensor. The advantageous mode mayfurther comprise, after the step of time propagating the covariance of astate vector comprising the position and velocity states of the target,the step of determining if target position information is available, andif target position is available, performing the further steps ofcalculating gain of the Kalman filter to generate Kalman filter gain.This advantageous mode may further comprise, after the step ofcalculating gain of the Kalman filter to generate Kalman filter gain,the step of generating updates of the state estimates of the target andof the covariance of the states.

A method for estimating the position of a target with the aid of asensor the position and angular orientation is not known accuratelyaccording to another aspect of the invention comprises the steps ofoperating the sensor to generate sensed information relating to theposition of the target, and adding to the sensed data any sensorpositional bias update information and sensor angular bias informationto produce updated sensed information. The state of the updated sensedinformation is propagated in time to produce time updated stateestimates of the target position and velocity. The Jacobian of the statedynamics of the target is computed. The state transition matrix for theextended Kalman filter algorithm is computed. The covariance of a statevector comprising the position and velocity states of the target ispropagated in time to thereby produce positional error information andangular bias information relating to the target tracking.

A method for estimating the position of a sensor and the angular biasorientation of a sensor according to another aspect of the inventioncomprises the steps of operating a sensor to generate sensed datarelating to a target, which data is contaminated by sensor positionalbias errors and angular bias registration errors, and adding to thesensed data any sensor positional bias update information and sensorangular bias update information to produce updated sensed information.The state of the updated sensed information is propagated in time toproduce time updated state estimates of the target position andvelocity. The Jacobian of the state dynamics of the target is computed.The state transition matrix for the extended Kalman filter algorithm iscomputed. The covariance of a state vector comprising the position andvelocity states of the target is time propagated.

A method for estimating the error in the position of a sensor and theangular bias orientation of the sensor according to another aspect ofthe invention comprises the steps of operating a sensor to generate datarelating to a target, which data is contaminated by sensor positionalbias errors and angular bias registration errors. The method comprisesthe steps of operating the sensor to generate sensed informationrelating to the target, and adding to the sensed data any sensorpositional bias update information and sensor angular bias updateinformation to produce updated sensed information. The state of theupdated sensed information is propagated in time to produce time updatedstate estimates of the target position and velocity. The Jacobian of thestate dynamics of the target is computed. The state transition matrixfor the extended Kalman filter algorithm is computed. The covariance ofa state vector comprising the position and velocity states of the targetis propagated in time to thereby produce positional error informationrelating to the target.

BRIEF DESCRIPTION OF THE DRAWING

FIG. 1 is a representation of a scenario in which a target is tracked bytwo or more spaced-apart sensors;

FIG. 2 is a notional representation of a system according to an aspectof the invention;

FIG. 3 is a representation of transformations among stable and biasedcoordinate systems;

FIG. 4 is a functional block diagram illustrating operation of a systemaccording to an aspect of the invention.

DESCRIPTION OF THE INVENTION

It would be desirable to be able to perform sensor tracking and fusionwith combined angular and positional registration bias estimationcapability independent of supplemental inputs such as GPS and orsatellite ephemeris. According to an aspect of the invention, amulti-sensor measurement fusion method produces fused ballistic inertialtarget track state estimates nominally free of navigational errorsattributable to angular and positional sensor registration biases. Amethod according to an aspect of the invention, given the name UnifiedNavigation and Inertial Tracking Estimation System (UNITES), may beimplemented with the aid of a computer algorithm. The methodincorporates an extended Kalman filter algorithm incorporating sensorregistration bias scheme requiring no ephemeris or GPS inputs (althoughthey may be used if desired). With sufficient computational resources,the sensor registration bias aspect operates in real time and providesreal-time or nominally instantaneous estimate of the angular andpositional sensor registration bias errors. The result is that thetarget is tracked with reduced errors attributable to sensor positionaland angular biases. This improved tracking, in turn, allows the targetto be attacked with a greater likelihood of success.

Angular sensor bias registration has been addressed through stateaugmentation, as described in U.S. patent application Ser. No.11/149,692, filed Jun. 10, 2005 in the name of Boka et al. and entitled“Instantaneous Multisensor Angular Bias Autoregistration” (ISAAC), and“System Calibration using Satellites (SCUS)” represented by U.S. Pat.No. 5,729,234, entitled “Remote alignment system” issued Mar. 17, 1998in the name of Stetson et al. Sensor positional bias registration erroramelioration or correction is described in U.S. patent application Ser.No. 11/504,561 and entitled “Method for Compensating for the PositionalErrors of a Sensor,” (GPSLess) filed on or about Aug. 14, 2006 in thename of Mookerjee et al.

The method of the present invention in effect merges the capabilities ofboth ISAAC and GPSLess by new algorithms which enable the angular andpositional sensor registration bias errors to be observed separately.FIG. 2 is a simplified notional representation 200 of the uniting ofISAAC with GPSLess to produce an improved UNITES result. In FIG. 2,representation 200 includes a source, illustrated as a block 210, ofmulti-sensor information representing information about the target(s).This information may be updated occasionally or periodically. Theinformation is made available from sensor block 210 to the UnifiedNavigation and Inertial Estimation System (UNITES), illustrated as ablock 212. UNITES block 212 includes fusion and estimation algorithms ina block 214. The fusion and estimation algorithms fuse or combine somethe angular registration bias features of provided by ISAAC 216 withpositional registration bias features of GPSLess 218. These fused orcombined features are combined with sensor registration processesillustrated as a block 220 and with target track state processingillustrated as a block 222 to produce (a) multisensor positional biasregistration bias estimates, (b) multisensor angular registration biasestimates, and (c) target track state and covariance estimates. Themultisensor positional bias registration bias estimates, multisensorangular registration bias estimates, and target track state andcovariance estimates are, in turn, used by combat and weapon systems(not illustrated), such as those aboard ships 1 and or 2 of FIG. 1, tomitigate the effects of sensor registration biases in the overall firecontrol process.

FIG. 3 represents coordinate frame topology and vector definitions foranalysis, depicted or couched in terms of true and nominal (biased)coordinate frame topologies. These conventions are consistent with Aegissea-based systems without loss of generality. In FIG. 3, starting at theleft end of the upper (Truth) transformation line, the E framerepresents the stable space frame, that is, Earth-Centered Earth-Fixed(ECEF), East-North Up (ENU), or Earth-Centered Inertial (ECI). Theearth's angular velocity vector ω_(E) is known exactly in this frame.The estimate {circumflex over (R)} _(E) of the sensor location vector R_(E) is known in this frame, where the underscore represents a vectorquantity, and the overcaret (^) represents an estimate. The estimate{circumflex over (R)} _(E) differs from the actual location or positionbased onR _(E) ={circumflex over (R)} _(E) +δ{circumflex over (R)} _(E)  (1)The matrix T_(E) ^(P)(t) represents the coordinate transformation fromthe E frame to the P frame, where (t) denotes a time dependency. The Pframe represents the platform frame (e.g. Aegis ship deck frame) thatmay be moving (e.g. wave motion) relative to the E stable space frame.The matrix T_(P) ^(S)(θ) represents the coordinate transformation fromthe P or platform frame to the S or sensor frame, where θ denotes theactual (unknown) rotation angles between P and S. The S sensor framerepresents the measurement sensor coordinate system (i.e. measurementvector X_(S) ^(m) known exactly in this frame). Note that δ{circumflexover (R)} _(E) is the estimated sensor registration position biasrepresented in the E frame.

The bottom line of FIG. 3, which is the nominal or biased transformationline, represents the nominal (biased) coordinate reference systemtransformations. The {tilde over (E)} frame represents the biased stablespace frame (e.g. biased ECEF, ENU, or ECI) that a filter wouldmistakenly work within after transforming the measurement vector X_(S)^(m) from the S sensor frame to the expected stable frame E using thenominal coordinate transformation matrices T_({tilde over (P)})^(S)({tilde over (θ)}) and T_({tilde over (E)})^({tilde over (S)})(t)(=T_(E) ^(S)(t)) (These last two coordinate frametransformations refer to the transformation between the frames asdenoted by the subscript and superscript of each term) where {tilde over(P)} denotes the nominal (biased) platform frame, and the overtilde (˜)represents a bias. Note that the biased platform transformation matrixT_(P) ^({tilde over (P)})(δ{tilde over (θ)}) can be represented as abias in the E frame using the similarity transformation

$\begin{matrix}{T_{E}^{\overset{\sim}{E}} = {{T_{P}^{E}(t)}{T_{P}^{\overset{\sim}{P}}\left( {\delta\;\overset{\sim}{\underset{\_}{\theta}}} \right)}{T_{E}^{P}(t)}}} & (2)\end{matrix}$

It should be noted that there may be a bias between the E and P framesof FIG. 3, which may be taken into account if the bias is significant.If the bias is significant, the UNITES state vector may be augmented toinclude that bias, as by using a modeling technique similar to that usedfor θ. For simplicity of description, however, and without the loss ofgenerality, the transformation from the E frame to the P frame is takenas being un-biased and only time dependent.

FIG. 4 is a simplified functional block diagram illustrating theoperation of an extended Kalman filter system according to an aspect ofthe invention. In FIG. 4, the process begins with application to aninput port 410 of an initial position measurement X_(S) ^(m) from asensor (not illustrated in FIG. 4) tracking a ballistic target. In orderto develop the filter equations, a model is assumed in which the targetis either ballistic (falling under the force of gravity) or else thespecific force (i.e. thrust acceleration) is known exactly and can becompensated for. Additionally, it is assumed that atmospheric drageffects are negligible due to the high altitudes at which targettracking occurs, or that atmospheric drag can be properly compensatedfor. Given these assumptions, the following equations describe the modelused for the target kinematics:

$\begin{matrix}\begin{matrix}{{\underset{\_}{\overset{¨}{X}}}_{E} = {\frac{{- \mu}{\underset{\_}{Z}}_{E}}{{{\underset{\_}{Z}}_{E}}^{3}} + {\underset{\_}{A}}_{E} - {{\underset{\_}{\omega}}_{E} \times \left( {{\underset{\_}{\omega}}_{E} \times {\underset{\_}{Z}}_{E}} \right)} - {2\;{\underset{\_}{\omega}}_{E} \times {\overset{.}{\underset{\_}{X}}}_{E}}}} \\{{\underset{\_}{Z}}_{E} = {{\underset{\_}{R}}_{E} + {\underset{\_}{X}}_{E}}}\end{matrix} & (3)\end{matrix}$where:

μ is the Earth gravitational constant;

A_(E) is the known specific force which includes (but is not limited to)effects such as higher order gravitational effects; and

X_(E) is the sensed target position reported in the E frame.

For simplicity of explanation, and without loss of generality, theremaining description omits the A_(E) term for conciseness and includesonly the dominant gravity term μ.

The initial sensor-generated position vector X_(S) ^(m) at time t_(m),representing the position of the target, is applied from port 410 ofFIG. 4 to an input port 412 i 1 of a measurement position bias updatefunction illustrated as a block 412. Block 412 sets the initial value ofthe estimated sensor position vector in the stable space frame({circumflex over (R)} _(E)) to the nominal bias value ({tilde over (R)}_(E)), {circumflex over (R)} _(E)={tilde over (R)} _(E) Measurementposition bias update function block 412 also updates the estimatedsensor position vector using the sensor position bias error estimateapplied to second input port 412 i 2 from the previous UNITES biasestimation event or iteration. Note that block 412 performs thisfunction at every cycle, not just at filter start-up. From block 412,the logic of system 400 of FIG. 4 flows to an input 414 i 1 of ameasurement angular bias update block 414. Block 414 initializes themeasurement at time t_(m) using the nominal value for alignment biases{circumflex over (θ)}={tilde over (θ)}. The measurement angular biasupdate function block 414 also receives at its second input port 414 i 2the sensor alignment bias error estimate δ{tilde over (θ)} from theprevious UNITES bias estimation event, and updates the sensor alignmentbias δ{tilde over (θ)} for the next cycle.

From block 414, the logic 400 of FIG. 4 flows to a measurement update ornext update cycle function represented by a block 420. Block 420determines whether the state and covariance estimates for the currenttime iteration will include a measurement update. The UNITES function400 of FIG. 4 nominally operates at a predetermined cycle rate, withmeasurement updates occurring asynchronously as they are available. Ifupdates are not available, block 420 merely sends the logic on to astate propagation function represented by a block 422.

The target state vector s is:

$\begin{matrix}{\underset{\_}{s} = \begin{Bmatrix}{\underset{\_}{X}}_{\overset{\sim}{E}} \\{\underset{\_}{\overset{.}{X}}}_{\overset{\sim}{E}} \\{\delta{\underset{\_}{R}}_{\overset{\sim}{E}}} \\{\delta\;\underset{\_}{\theta}}\end{Bmatrix}} & (4)\end{matrix}$State vector s includes target position, velocity, and positional andangular registration biases for all reporting sensors. Position andvelocity vectors are referenced relative to the biased stable spaceframe {tilde over (E)}. The angular registration bias state vector δθ isreferenced relative to the platform frame P, and the positionalregistration bias δR _(E) is referenced relative to the biased stablespace frame and is constant in that frame.

The dynamics equations (i.e. the nonlinear state derivative equations)are set forth in equations (5). These equations are based on the assumedtarget kinematics described in relation to equation (3). Additionally,it is assumed that all sensor positional and angular registration biasesare constant.

$\begin{matrix}{\underset{\_}{\overset{.}{s}} = {\begin{Bmatrix}{\overset{.}{\underset{\_}{X}}}_{\overset{\sim}{E}} \\{\underset{\_}{\overset{¨}{X}}}_{\overset{\sim}{E}} \\{\delta{\underset{\_}{\overset{.}{R}}}_{\overset{\sim}{E}}} \\{\delta\;\overset{.}{\underset{\_}{\theta}}}\end{Bmatrix} = \begin{Bmatrix}\begin{matrix}\begin{matrix}{\overset{.}{\underset{\_}{X}}}_{\overset{\sim}{E}} \\{\frac{{- \mu}\;{\underset{\_}{Z}}_{\overset{\sim}{E}}}{{{\underset{\_}{Z}}_{\overset{\sim}{E}}}^{3}} - {{\underset{\_}{\omega}}_{\overset{\sim}{E}} \times \left( {{\underset{\_}{\omega}}_{\overset{\sim}{E}} \times {\underset{\_}{Z}}_{\overset{\sim}{E}}} \right)} - {2{\underset{\_}{\omega}}_{\overset{\sim}{E}} \times {\underset{\_}{\overset{.}{X}}}_{\overset{\sim}{E}}}}\end{matrix} \\{\underset{\_}{0}}_{3 \times 1}\end{matrix} \\{\underset{\_}{0}}_{3 \times 1}\end{Bmatrix}}} & (5)\end{matrix}$

From block 420 of FIG. 4, the logic 400 flows to a state propagationfunction illustrated as a block 422. The state propagation block 422 oflogic 400 of FIG. 4 propagates (in time) the UNITES state vector inequation (4) to produce time updated state estimates of the targetposition and velocity. Time propagation of the state vector is performedin block 422 by numerically integrating the state derivative vector fromthe previous time t_(i-1), to the current time t_(i), where thesubscript i refers to the filter cycle iteration:

$\begin{matrix}{{\hat{\underset{\_}{s}}\left( t_{i} \right)} = {{\hat{\underset{\_}{s}}\left( t_{i - 1} \right)} + {\int_{t_{i - 1}}^{t_{i - 1 + {\Delta\; t}}}{{\underset{\_}{\hat{\overset{.}{s}}}(\tau)}\ {\mathbb{d}\tau}}}}} & (6)\end{matrix}$For the integration process, a high order numerical integrationalgorithm, such as the 2^(nd) order or 4^(th) order Runge Kuttaalgorithm might be used. The incremental time step Δt refers to eitherthe nominal update cycle time or the incremental time step from the lastcycle time to the current measurement time t_(m) (i.e.,Δt=t_(m)−t_(i-1)) At the first iteration, the state propagation functionof block 422 also initializes the state for the subsequent iterations.From state propagation block 422, the logic of FIG. 4 flows to aJacobian computation represented by a block 424.

The Jacobian computation function 424 of FIG. 4 computes the Jacobian ofthe state dynamics of the target, and subsequently computes, from thestate dynamics of the target, the state transition matrix for the UNITESextended Kalman filter algorithm. The UNITES Jacobian matrix formulationof the state dynamics provides observability into the sensor alignmentbias through the gravitational and coriolis forces. The Jacobian J ofthe state dynamics equation is given by

$\begin{matrix}{J = {\left\lbrack \frac{\partial\overset{.}{\underset{\_}{s}}}{\partial\underset{\_}{s}} \right\rbrack = {\left\lbrack {\frac{\partial\underset{\_}{\overset{.}{s}}}{\partial{\underset{\_}{X}}_{\overset{\sim}{E}}}\frac{\partial\underset{\_}{\overset{.}{s}}}{\partial{\overset{.}{\underset{\_}{X}}}_{\overset{\sim}{E}}}\frac{\partial\underset{\_}{\overset{.}{s}}}{{\partial\delta}{\underset{\_}{R}}_{\overset{\sim}{E}}}\frac{\partial\overset{.}{\underset{\_}{s}}}{{\partial\delta}\underset{\_}{\theta}}} \right\rbrack = \left\lbrack \begin{matrix}0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\\frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{\partial{\underset{\_}{X}}_{\overset{\sim}{E}}} & \frac{\partial\overset{¨}{{\underset{\_}{X}}_{\overset{\sim}{E}}}}{\partial\overset{.}{{\underset{\_}{X}}_{\overset{\sim}{E}}}} & \frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{{\partial\delta}{\underset{\_}{R}}_{\overset{\sim}{E}}} & \frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{{\partial\delta}\underset{\_}{\theta}} \\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\end{matrix} \right\rbrack}}} & (7)\end{matrix}$where:

$\begin{matrix}{\frac{\partial{\underset{\_}{\overset{¨}{X}}}_{\overset{\sim}{E}}}{\partial{\underset{\_}{X}}_{\overset{\sim}{E}}} = {{- {\frac{\mu}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)^{\frac{3}{2}}}\left\lbrack {I_{3 \times 3} - {\frac{3}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)}\left\lbrack {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}^{T}} \right\rbrack}} \right\rbrack}} - {{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}}}} \\{\frac{\partial{\underset{\_}{\overset{¨}{X}}}_{\overset{\sim}{E}}}{\partial{\overset{.}{\underset{\_}{X}}}_{\overset{\sim}{E}}} = {{- 2} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}}} \\{\frac{\partial\overset{¨}{\underset{\_}{X}}}{{\partial\delta}{\underset{\_}{R}}_{E}} = 0_{3 \times 1}} \\{{\frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{{\partial\delta}\underset{\_}{\theta}} = {{{- {\frac{\mu}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)^{\frac{3}{2}}}\left\lbrack {I_{3 \times 3} - {\frac{3}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)}\left\lbrack {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}^{T}} \right\rbrack}} \right\rbrack}} \cdot {〚{\hat{\underset{\_}{R}}}_{\overset{\sim}{E}}〛} \cdot {{\hat{T}}_{\overset{\sim}{p}}^{\overset{\sim}{E}}\left( t_{i} \right)}} - {2 \cdot \left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\overset{\sim}{E}} \right\rbrack \right\rbrack \cdot \mspace{101mu}{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {{\hat{T}}_{\overset{\sim}{p}}^{\overset{\sim}{E}}\left( t_{i} \right)}} + {{\left\lbrack {{{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{X}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} + {{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{R}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} + \mspace{70mu}{{〚{{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {\underset{\_}{\hat{X}}}_{\overset{\sim}{E}}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} + {{〚{{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {\underset{\_}{\hat{R}}}_{\overset{\sim}{E}}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} - {{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{R}}}_{\overset{\sim}{E}}〛}}} \right\rbrack \cdot {{\hat{T}}_{\overset{\sim}{p}}^{\overset{\sim}{E}}\left( t_{i} \right)}}\mspace{14mu}{and}}}}\begin{matrix}{{\hat{\underset{\_}{R}}}_{\overset{\sim}{E}} = {{\hat{T}}_{E}^{\overset{\sim}{E}} \cdot {\underset{\_}{\hat{R}}}_{E}}} \\{{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}} = {{\hat{T}}_{E}^{\overset{\sim}{E}} \cdot {\underset{\_}{\omega}}_{E}}} \\{{\hat{T}}_{E}^{\overset{\sim}{E}} = {{T_{P}^{E}\left( t_{i} \right)} \cdot \left\lbrack {I_{3 \times 3} + {〚\hat{\underset{\_}{\theta}}〛}} \right\rbrack \cdot {T_{E}^{P}\left( t_{i} \right)}}}\end{matrix}}\end{matrix}$The

•

notation denotes a skew symmetric matrix of the vector argument.The state transition matrix used for the time propagation of the UNITESerror covariance can be approximated including up to 2^(nd) order terms:Φ≈I+JΔt+0.5J ² Δt ²  (8)

From Jacobian computation block 424, the logic 400 of FIG. 4 flows to ablock 426, which represents the use of the state transition matrix totime propagate the covariance of a state vector comprising the positionand velocity states of the target. The covariance propagation functionperformed by block 426 is the traditional Kalman filter time propagationof the state covariance matrix utilizing the state transition matrix.This function will nominally operate faster than the measurementprocessing to reduce non-linear effects when measurement updates occurat slow update rates. At the first iteration, the covariance propagationfunction of block 426 also initializes the state covariance for thesubsequent iterations. Time propagation of the UNITES error covariancematrix P(t_(i)) is performed with the following equation:P(t _(i))=ΦP(t _(i-1))Φ^(T) +Q  (9)where:

Q is the UNITES state noise matrix.

The state noise matrix Q may be determined using the following equation:

$\begin{matrix}{Q = {\int_{0}^{\Delta\; t}{\Phi\; W\;\Phi^{T}\ {\mathbb{d}\tau}}}} & (10)\end{matrix}$where:

W=E(w(τ)w(τ)^(T)); and

w(τ) is the 12×1 state noise vector of white noise.

From block 426 of FIG. 4, the logic 400 flows to a decision block 428.Decision block 428 determines if a position measurement is currentlyavailable. If a measurement is not currently available, then time isincremented by the nominal Δt propagation time rate, the logic leavesdecision block 428 by the “No” output, and control is passed by way of apath 430 to block 420 at the start of the iteration loop 406. If, on theother hand, a measurement is available, then the logic of FIG. 2 leavesdecision block 428 by the “Yes” output, and proceeds to a block 432,representing a gain computation.

The gain computation block 432 of FIG. 4 constructs the standard Kalmanfilter gain matrix K using the measurement matrix and the errorcovariance matrixK=P(t _(i))·H ^(T)·(H·P(t _(i))·H ^(T) +R)⁻¹  (11)where:H=[I _(3×3) 0_(3×3) I _(3×3) 0_(3×3)]  (12)

is the measurement matrix; and

R is the measurement noise covariance matrix associated with thecurrently reporting sensor defined in the stable space frame

$\begin{matrix}{R = {{{\hat{T}}_{P}^{E}\left( t_{i} \right)} \cdot {{\hat{T}}_{S}^{P}\left( \underset{\_}{\hat{\theta}} \right)} \cdot \Sigma \cdot {{\hat{T}}_{P}^{S}\left( \underset{\_}{\hat{\theta}} \right)} \cdot {{\hat{T}}_{E}^{P}\left( t_{i} \right)}}} & (13)\end{matrix}$with Σ denoting the measurement noise matrix as represented in thesensor frame.

It should be noted that the gain computation expressed by equations (11)and (12) is applicable only in the case in which the measurement updateis of the target position only. If the measurement update includes bothtarget position and target velocity information, the gain of the Kalmanfilter is generated in accordance with equation (11) with themeasurement matrix H redefined as

$\begin{matrix}{H = \begin{bmatrix}I_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} \\0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\end{bmatrix}} & (14)\end{matrix}$and including the effect of the velocity in the second row.

From gain computation block 432, the logic of FIG. 4 flows to a block434, representing the state measurement and state residuals updateincluding the sensor registration bias estimate. The state vector isupdated usingŝ=ŝ+K·Δm  (15)where:

the measurement residual Δm is defined as

$\begin{matrix}{{\Delta\underset{\_}{m}} = {{{{\hat{T}}_{P}^{E}\left( t_{i} \right)} \cdot {{\hat{T}}_{S}^{P}\left( \hat{\underset{\_}{\theta}} \right)} \cdot {{\underset{\_}{X}}_{S}^{m}\left( t_{i} \right)}} - {H \cdot \underset{\_}{s}}}} & (16)\end{matrix}$The updated state measurements are made available by way of path 440 tosensor positional bias update switch function 416M and sensor angularbias update switch function 418M, for inclusion in the next iteration.The updated state measurements produced by block 434 include the updatesto the target missile position X_({tilde over (E)}) and velocity {dotover (X)}_({tilde over (E)}) in the estimated bias frame {tilde over(Ê)}. The updated state measurements also include the update to thesensor biases for position δR _({tilde over (E)}) and angle δθ. Theupdated state measurements are made available by connections to path 440for use by an antimissile targeting arrangement, not illustrated in FIG.4. Note that, according to an advantageous result of this processing, asthe updated angular bias estimates converge to the true bias, the biasedframe converges toward the true sensor frame.

From block 434 of FIG. 4, the logic 400 flows to a further block 436,representing the updating of the covariance measurement. The covariancemeasurement update function of block 436 performs the measurement updateof the state covariance matrixP(t _(i))=(I−K·H)·P(t _(i))·(I−K·H)^(T) +K·R·K ^(T)  (17)which is made available by way of path 442 to a block 444 representing atransformation of the state and covariance to an estimated unbiasedframe Ê. The state covariance in estimated biased frame {tilde over (Ê)}is also made available for external use for fire control or guidancepurposes for missile targeting. The logic flow 400 continues withupdating of the time t=tm+Δt, and the logic returns by way of path 438to block 420 to begin another iteration around loop 406.

State and covariance transformation block 444 of FIG. 4 performstransformations of target missile position, velocity, and covariance tothe estimated stable space frame {tilde over (Ê)} using equations (18)

${\underset{\_}{\hat{X}}}_{\overset{.}{E}} = {T_{\overset{\sim}{E}}^{\overset{.}{E}} \cdot {\underset{\_}{\hat{X}}}_{\overset{\sim}{E}}}$${\underset{\_}{\hat{\overset{.}{X}}}}_{\overset{.}{E}} = {T_{\overset{\sim}{E}}^{\overset{.}{E}} \cdot {\underset{\_}{\hat{\overset{.}{X}}}}_{\overset{\sim}{E}}}$$P_{\overset{.}{X}\overset{.}{X}} = {P_{\overset{\sim}{X}\overset{\sim}{X}} + {{〚{\hat{\underset{\_}{X}}}_{\hat{E}}〛} \cdot {T_{P}^{\hat{E}}\left( t_{1} \right)} \cdot P_{\theta\theta} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot \left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\hat{E}} \right\rbrack \right\rbrack^{T}} + {T_{\overset{\sim}{E}}^{\hat{E}} \cdot P_{\overset{\sim}{\overset{.}{X}}\theta} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot \left\lbrack \left\lbrack {\underset{\_}{\hat{X}}}_{\hat{E}} \right\rbrack \right\rbrack^{T}} + {\left\lbrack \left\lbrack {\underset{\_}{\hat{X}}}_{\hat{E}} \right\rbrack \right\rbrack \cdot {T_{P}^{\hat{E}}\left( t_{i} \right)} \cdot P_{\overset{\sim}{X}\theta}^{T} \cdot T_{\hat{E}}^{\overset{\sim}{E}}}}$$P_{\hat{\overset{.}{X}}\hat{\overset{.}{X}}} = {P_{\overset{\sim}{\overset{.}{X}}\overset{\sim}{\overset{.}{X}}} + {\left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\hat{E}} \right\rbrack \right\rbrack \cdot {T_{P}^{\hat{E}}\left( t_{i} \right)} \cdot P_{\theta\theta} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot \left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\hat{E}} \right\rbrack \right\rbrack^{T}} + {T_{\overset{\sim}{E}}^{\hat{E}} \cdot P_{\overset{\sim}{\overset{.}{X}}\theta} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot \left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\hat{E}} \right\rbrack \right\rbrack^{T}} + {\left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\hat{E}} \right\rbrack \right\rbrack \cdot {T_{P}^{\hat{E}}\left( t_{i} \right)} \cdot P_{\overset{\sim}{\overset{.}{X}}\theta}^{T} \cdot T_{\hat{E}}^{\overset{\sim}{E}}}}$${{where}:{P\left( t_{i} \right)}} = \begin{bmatrix}P_{\overset{\sim}{X}\overset{\sim}{X}} & P_{\overset{\sim}{X}\overset{\sim}{\overset{.}{X}}} & P_{\overset{\sim}{X}\delta\; X} & P_{\overset{\sim}{X}\theta} \\P_{\overset{\sim}{X}\overset{\sim}{\overset{.}{X}}}^{T} & P_{\overset{\sim}{\overset{.}{X}}\overset{\sim}{\overset{.}{X}}} & P_{\overset{\sim}{\overset{.}{X}}\delta\; X} & P_{\overset{\sim}{\overset{.}{X}\theta}} \\P_{\overset{\sim}{X}\delta\; X}^{T} & P_{\overset{\sim}{\overset{.}{X}}\delta\; X}^{T} & P_{\delta\; X\;{\delta X}} & P_{\delta\; X\;\theta} \\P_{\overset{\sim}{X}\theta}^{T} & P_{\overset{\sim}{\overset{.}{X}}\theta}^{T} & P_{\delta\; X\;\theta}^{T} & P_{\theta\theta}\end{bmatrix}$${\hat{T}}_{E}^{\overset{\sim}{E}} = {{T_{P}^{E}\left( t_{i} \right)} \cdot \left\lbrack {I_{3 \times 3} + {{〚\hat{\underset{\_}{\theta}}〛} \cdot {T_{E}^{P}\left( t_{i} \right)}}} \right.}$The transformed position, velocity and covariance in the stable spaceframe are made available by way of paths 446 for use by an antimissiletargeting arrangement, not illustrated in FIG. 4.

At the end of each iteration through the logic 400 of FIG. 4, the sensorposition vector estimate {circumflex over (R)} _(E) is updated using{circumflex over (R)} _(E)={circumflex over (R)} _(E)+δ{circumflex over(R)} _(E). At initialization, the sensor position vector assumes thenominal value {circumflex over (R)} _(E)={tilde over (R)} _(E). Also atthe end of each iteration, the sensor angular bias estimate {circumflexover (θ)} is updated using {circumflex over (θ)}={circumflex over(θ)}+δ{circumflex over (θ)}. At initialization, the sensor angular biasestimate assumes the nominal value of {circumflex over (θ)}={tilde over(θ)}.

1. A method for compensating for the positional and alignment errors ofa sensor tracking a target with known acceleration, which sensorgenerates sensed information, said method comprising the steps of:defining the estimator state given by$\underset{\_}{s} = \begin{Bmatrix}{\underset{\_}{X}}_{\overset{\sim}{E}} \\{\underset{\_}{\overset{.}{X}}}_{\overset{\sim}{E}} \\{\delta\;{\underset{\_}{R}}_{\overset{\sim}{E}}} \\{\delta\underset{\_}{\;\theta}}\end{Bmatrix}$ comprising the target position X _({tilde over (E)}) andvelocity {dot over (X)} _({tilde over (E)}) and the positional bias δR_({tilde over (E)}) of the sensor and the angular bias of the sensor δθ;applying to said sensed information any sensor positional bias updateinformation and angular bias information to produce updated sensedinformation which ultimately provides improved target state information;propagating state estimates from the previous time t_(i-1) to thecurrent time t_(i) where the subscript i refers to the filter cycleiteration to produce time updated state estimates according to${{\underset{\_}{\hat{s}}\left( t_{i} \right)} = {{\underset{\_}{\hat{s}}\left( t_{i - 1} \right)} + {\int_{t_{i - 1}}^{t_{i - 1} + {\Delta\; 1}}{{\underset{\_}{\hat{\overset{.}{s}}}(\tau)}\ {\mathbb{d}\tau}}}}};$computing the Jacobian of the state dynamics of the target according to$J = {\left\lbrack \frac{\partial\underset{\_}{\overset{.}{s}}}{\partial\underset{\_}{s}} \right\rbrack = {\left\lbrack {\frac{\partial\underset{\_}{\overset{.}{s}}}{\partial{\underset{\_}{X}}_{\overset{\sim}{E}}}\frac{\partial\underset{\_}{\overset{.}{s}}}{\partial{\underset{\_}{\overset{.}{X}}}_{\overset{\sim}{E}}}\frac{\partial\overset{.}{\underset{\_}{s}}}{{\partial\delta}{\underset{\_}{R}}_{\overset{\sim}{E}}}\frac{\partial\overset{.}{\underset{\_}{s}}}{{\partial\delta}\underset{\_}{\theta}}} \right\rbrack = \begin{bmatrix}0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\\frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{\partial{\underset{\_}{X}}_{\overset{\sim}{E}}} & \frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{\partial{\overset{.}{\underset{\_}{X}}}_{\overset{\sim}{E}}} & \frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{{\partial\delta}{\underset{\_}{R}}_{\overset{\sim}{E}}} & \frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{{\partial\delta}\underset{\_}{\theta}} \\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} \\0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\end{bmatrix}}}$ where: $\begin{matrix}{\frac{\partial{\underset{\_}{\overset{¨}{X}}}_{\overset{\sim}{E}}}{\partial{\underset{\_}{X}}_{\overset{\sim}{E}}} = {{- {\frac{\mu}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)^{\frac{3}{2}}}\left\lbrack {I_{3 \times 3} - {\frac{3}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)}\left\lbrack {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}^{T}} \right\rbrack}} \right\rbrack}} - {{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}}}} \\{\frac{\partial{\underset{\_}{\overset{¨}{X}}}_{\overset{\sim}{E}}}{\partial{\overset{.}{\underset{\_}{X}}}_{\overset{\sim}{E}}} = {{- 2} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}}} \\{\frac{\partial\overset{¨}{\underset{\_}{X}}}{{\partial\delta}{\underset{\_}{R}}_{E}} = 0_{3 \times 1}} \\{{\frac{\partial{\overset{¨}{\underset{\_}{X}}}_{\overset{\sim}{E}}}{{\partial\delta}\underset{\_}{\theta}} = {{{- {\frac{\mu}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)^{\frac{3}{2}}}\left\lbrack {I_{3 \times 3} - {\frac{3}{\left( {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}} \right)}\left\lbrack {{\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}} \cdot {\hat{\underset{\_}{Z}}}_{\overset{\sim}{E}}^{T}} \right\rbrack}} \right\rbrack}} \cdot {〚{\hat{\underset{\_}{R}}}_{\overset{\sim}{E}}〛} \cdot {{\hat{T}}_{\overset{\sim}{p}}^{\overset{\sim}{E}}\left( t_{i} \right)}} - {2 \cdot \left\lbrack \left\lbrack {\underset{\_}{\hat{\overset{.}{X}}}}_{\overset{\sim}{E}} \right\rbrack \right\rbrack \cdot \mspace{101mu}{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {{\hat{T}}_{\overset{\sim}{p}}^{\overset{\sim}{E}}\left( t_{i} \right)}} + {\left\lbrack {{{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{X}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} + {{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{R}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} + \mspace{70mu}{{〚{{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {\underset{\_}{\hat{X}}}_{\overset{\sim}{E}}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} + {{〚{{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {\underset{\_}{\hat{R}}}_{\overset{\sim}{E}}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛}} - {{〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{\omega}}}_{\overset{\sim}{E}}〛} \cdot {〚{\underset{\_}{\hat{R}}}_{\overset{\sim}{E}}〛}}} \right\rbrack \cdot {{\hat{T}}_{\overset{\sim}{p}}^{\overset{\sim}{E}}\left( t_{i} \right)}}}}\;{{and}\mspace{14mu}\underset{\_}{where}}\text{}\begin{matrix}{{\underset{\_}{\hat{R}}}_{\overset{\sim}{E}} = {{\hat{T}}_{E}^{\overset{\sim}{E}} \cdot {\underset{\_}{\hat{R}}}_{E}}} \\{{\hat{\underset{\_}{\omega}}}_{\overset{\sim}{E}} = {{\hat{T}}_{E}^{\overset{\sim}{E}} \cdot {\underset{\_}{\omega}}_{E}}} \\{{\hat{T}}_{E}^{\overset{\sim}{E}} = {{T_{P}^{E}\left( t_{i} \right)} \cdot \left\lbrack {I_{3 \times 3} + {〚\underset{\_}{\hat{\theta}}〛}} \right\rbrack \cdot {T_{E}^{P}\left( t_{i} \right)}}}\end{matrix}}\end{matrix}$ and where J is the Jacobian of the state dynamics of thetarget; s represents the sensor frame; underscore (_) represent a vectorquantity; overcaret (^) represents an estimate of the argument;overtilde (˜) represents a bias term; ( )^(T) represents the transposeof the argument matrix; 0_(3×3) is a 3×3 matrix in which all componentsare zero; I_(3×3) is a 3×3 identity matrix (ones along the principaldiagonal and zeroes elsewhere); Z is the vector sum of the sensorposition vector with respect to the center of the Earth, and therelative position vector of the sensed object with respect to the sensorposition; μ is the Earth's gravitational constant; ω is the Earth'sangular velocity vector; T represents a coordinate system transformationmatrix; P represents a covariance matrix of an estimated state vector; Qrepresents the process noise covariance matrix used by a Kalman filter;and the

•

notation denotes a skew symmetric matrix of the vector argument;computing, from said Jacobian, the state transition matrix for theextended Kalman filter algorithmΦ≈I+JΔt+0.5J ² Δt ² where Φ is the state transition matrix truncated toinclude no more than second-order terms; I is a 12×12 identity matrix; Jis a 12×12 Jacobian matrix; and using said state transition matrix, timepropagating the covariance of a state vector comprising the position andvelocity states of said target and the positional bias of the sensorgiven byP(t _(i))=ΦP(t _(i-1))Φ^(T) +Q.
 2. A method for target tracking, saidmethod comprising the steps of: sensing a target with at least onesensor subject to positional bias and angular bias, to thereby producetarget representative sensed signals subject to sensor positional andangular biases; updating said sensor positional bias and said sensorangular bias with any sensor positional bias updates and sensor angularbias updates, respectively, to thereby produce sensor positional-biasand sensor angular-bias updated target representative sensed signals;non-linear time propagating the state that includes at least sensorpositional-bias and sensor angular-bias sensed signals and the targetpositional and velocity states of said sensor positional-bias and sensorangular-bias updated target representative signals to produce timeupdated target states and sensor positional and angular biases;computing the Jacobian of the state dynamics of a target model toproduce the state transition matrix for extended Kalman filtering; timepropagating the covariance of a state vector comprising at leastposition and velocity of said target and positional and angular bias ofsaid sensor, to thereby produce time updated state covariance;determining if a sensor measurement update is available; if said sensormeasurement update is not available, repeating said steps of state timepropagating, computing the Jacobian, and covariance time propagating; ifsaid sensor measurement update is available, determining the Kalmanfilter gain; determining the Kalman measurement residual comprising (a)the time updated target states and sensor positional and angular biasesand (b) the sensor measurements; weighting said Kalman measurementresidual with said gain to produce state correction, and adding saidstate correction to said (a) the time updated target states and sensorpositional and angular biases and (b) the sensor measurements to therebyproduce (i) target position and velocity state updates and (ii) saidsensor positional bias updates and sensor angular bias updates; usingsaid Kalman gain, covariance measurement propagating the covariance of astate vector comprising at least position and velocity of said targetand positional and angular bias of said sensor, to thereby producemeasurement updated state covariance consisting of (i) target positionand velocity measurement covariance updates and (ii) said sensorpositional bias measurement covariance updates and sensor angular biasmeasurement covariance updates.
 3. A method according to claim 2,further comprising the step of transformation of coordinates of thestate vector and state covariance of said Kalman filter wherein saidtransformation of coordinates is between the sensor bias filter frameand an unbiased filter frame.
 4. A method according to claim 3, whereinsaid step of transformation of coordinates is performed using$\left. \left. {\left. \left. {{\left. \left. {{\left. \left. {\left. \left. {\left. \left. {{\left. \left. {{\hat{{\underset{\_}{X}}_{\hat{E}}} = {T_{\overset{\sim}{E}}^{\hat{E}} \cdot \hat{{\underset{\_}{X}}_{\overset{\sim}{E}}}}}{{\hat{\overset{.}{\underset{\_}{X}}}}_{\hat{E}} = {T_{\overset{\sim}{E}}^{\hat{E}} \cdot {\hat{\overset{.}{\underset{\_}{X}}}}_{\overset{\_}{E}}}}{P_{\hat{X}\hat{X}} = {P_{\overset{\sim}{X}\overset{\sim}{X}} + {{\hat{\underset{\_}{X}}}_{\hat{E}}}}}} \right\rbrack \right\rbrack \cdot {T_{P}^{\hat{E}}\left( t_{1} \right)} \cdot P_{00} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot}{\hat{\underset{\_}{X}}}_{\hat{E}}} \right\rbrack \right\rbrack^{T} + {{T_{\overset{\sim}{E}}^{\hat{E}} \cdot P_{\overset{\_}{X}0} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot}{\hat{\underset{\_}{X}}}_{\hat{E}}}} \right\rbrack \right\rbrack^{T} + {{\hat{\underset{\_}{X}}}_{\hat{E}}}} \right\rbrack \right\rbrack{{\quad\quad} \cdot {T_{P}^{\hat{E}}\left( t_{i} \right)} \cdot P_{\overset{\sim}{X}0}^{T} \cdot T_{\overset{.}{E}}^{\overset{\sim}{E}}}}{P_{\hat{\overset{.}{X}}\hat{\overset{.}{X}}} = {P_{\overset{\sim}{\overset{.}{X}}\overset{\sim}{\overset{.}{X}}} +^{\;}{{\hat{\overset{.}{X}}}_{\hat{E}}}}}} \right\rbrack \right\rbrack \cdot {T_{P}^{\hat{E}}\left( t_{1} \right)} \cdot P_{00} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot}{\hat{\underset{\_}{\overset{.}{X}}}}_{\hat{E}}} \right\rbrack \right\rbrack^{T} + {{{T_{\overset{\sim}{E}}^{\hat{E}}}^{\;} \cdot P_{\overset{\_}{\overset{.}{X}}0} \cdot {T_{\hat{E}}^{P}\left( t_{i} \right)} \cdot}\hat{{\underset{\_}{\overset{.}{X}}}_{\hat{E}}}}} \right\rbrack \right\rbrack \cdot {T_{P}^{\hat{E}}\left( t_{i} \right)} \cdot P_{\overset{\_}{\overset{.}{X}}0}^{T} \cdot T_{\hat{E}}^{\overset{\sim}{E}}$where: underscore (_) represent a vector quantity; overcaret (^)represents an estimate of the argument; overtilde (˜) represents a biasterm; [[•]] denotes a skew symmetric matrix of the vector argument;[[•]]^(T) denotes the transpose of a skew symmetric matrix of the vectorargument; {circumflex over (X)}_({tilde over (E)}) is the estimatedtarget position vector represented in the biased frame;T_({tilde over (E)}) ^(Ê) is the coordinate transformation from the truebiased frame to the estimated biased frame; {dot over ({circumflex over(X)}_(Ê) is the estimated target velocity vector represented in thebiased frame; {dot over ({circumflex over (X)}_({tilde over (E)}) is theestimated target velocity vector represented in the estimated biasedframe; P_({circumflex over (X)}{circumflex over (X)}) is the 3×3covariance matrix of the target position vector with respect to theestimated biased frame; P_({tilde over (X)}{tilde over (X)}) is the 3×3covariance matrix in the true bias frame; T_(P) ^(Ê)(t_(i)) is thecoordinate transformation from the platform frame to the estimatedbiased frame at time t_(i); P_(θθ) is the 3×3 covariance matrix of theestimated angular bias error with respect to the true biased frame;P_({tilde over (x)}θ) is the 3×3 cross-covariance matrix between thetarget position state and the angular biased state with respect to thetrue biased frame; T_(Ê) ^(P)(t_(i)) is the transpose of T_(P)^(Ê)(t_(i)); ${P\left( t_{i} \right)} = {\begin{bmatrix}P_{\overset{\sim}{X}\overset{\sim}{X}} & P_{\overset{\sim}{X}\overset{\sim}{\overset{.}{X}}} & P_{\overset{\sim}{X}\delta\; X} & P_{\overset{\sim}{X}\theta} \\P_{\overset{\sim}{X}\overset{\sim}{\overset{.}{X}}}^{T} & P_{\overset{\sim}{\overset{.}{X}}\overset{\sim}{\overset{.}{X}}} & P_{\overset{\sim}{\overset{.}{X}}\delta\; X} & P_{\overset{\sim}{\overset{.}{X}\theta}} \\P_{\overset{\sim}{X}\delta\; X}^{T} & P_{\overset{\sim}{\overset{.}{X}}\delta\; X}^{T} & P_{\delta\; X\;{\delta X}} & P_{\delta\; X\;\theta} \\P_{\overset{\sim}{X}\theta}^{T} & P_{\overset{\sim}{\overset{.}{X}}\theta}^{T} & P_{\delta\; X\;\theta}^{T} & P_{\theta\theta}\end{bmatrix}\mspace{14mu}{and}}$${\hat{T}}_{E}^{\overset{\sim}{E}} = {{T_{P}^{E}\left( t_{i} \right)} \cdot \left\lbrack {I_{3 \times 3} + {\cdot {{T_{E}^{P}\left( t_{i} \right)}.}}} \right.}$5. A method according to claim 2, wherein said step of determining theKalman filter gain comprises the step of calculating the Kalman filtergain matrixK=P(t _(i))H ^(T) [HP(t _(i))H ^(T) +R] ⁻¹ where: •^(T) represents thetranspose of the argument; P(t_(i)) is a covariance matrix of anestimated state vector evaluated at a time i; H=[I_(3×3) 0_(3×3) I_(3×3)0_(3×3)] is the measurement matrix; and R is the measurement noisecovariance matrix associated with the currently reporting sensor.
 6. Amethod according to claim 2, further comprising, after said step ofdetermining the Kalman filter gain, the steps of: generating updates ofthe states of said target and of the covariance of said states of saidtarget as followsŝ=ŝ+Δm where: the measurement residual Δm is defined as${\Delta\underset{\_}{m}} = {{{\hat{T}}_{P}^{E} \cdot {{\hat{T}}_{S}^{P}\left( \underset{\_}{\hat{\theta}} \right)} \cdot {\underset{\_}{X}}_{S}^{m}} - {H \cdot \underset{\_}{s}}}${circumflex over (T)}_(P) ^(E) is the coordinate transformation matrixfrom the platform frame to the estimated bias frame; {circumflex over(T)}_(S) ^(P)({circumflex over (θ)}) is the coordinate transformationmatrix from the sensor frame to the platform frame based on theestimated angular bias X_(S) ^(m) the target position measurement vectorwith respect to the sensor frame; H=[I_(3×3) 0_(3×3) I_(3×3) 0_(3×3)] isthe measurement matrix.
 7. A method according to claim 4, furthercomprising, after said step of time propagating the covariance of astate vector comprising the position and velocity states of said target,the step of determining if target position and target velocityinformation are available, and if target position and target velocityare available, performing the further steps of: calculating gain of theKalman filter to generate Kalman filter gain in accordance withK=P(t _(i))H ^(T) [HP(t _(i))H ^(T) +R] ⁻¹ where: P(t_(i)) is acovariance matrix of an estimated state vector evaluated at a time i;$H = \begin{bmatrix}I_{3 \times 3} & 0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} \\0_{3 \times 3} & I_{3 \times 3} & 0_{3 \times 3} & 0_{3 \times 3}\end{bmatrix}$ is the measurement matrix; and R is the measurement noisecovariance matrix associated with the currently reporting sensor.
 8. Amethod for estimating the position of a target with the aid of a sensorthe position and angular orientation of which are not known accurately,said method comprising the steps of: operating said sensor to generatesensed information relating to the position of said target; adding tosaid sensed data any sensor positional bias update information andsensor angular bias information to produce updated sensed information;propagating the state of said updated sensed information to produce timeupdated state estimates of the target position and velocity; computingthe Jacobian of the state dynamics of the target; computing the statetransition matrix for the extended Kalman filter algorithm; and timepropagating the covariance of a state vector comprising the position andvelocity states of said target to thereby produce positional errorinformation and angular bias information relating to said targettracking.
 9. A method for estimating the position of a sensor and theangular bias orientation of a sensor, said method comprising the stepsof: operating a sensor to generate sensed data relating to a target,said data being contaminated by sensor positional bias errors andangular bias registration errors; adding to said sensed data any sensorpositional bias update information and sensor angular bias updateinformation to produce updated sensed information; propagating the stateof said updated sensed information to produce time updated stateestimates of the target position and velocity; computing the Jacobian ofthe state dynamics of the target; computing the state transition matrixfor the extended Kalman filter algorithm; and time propagating thecovariance of a state vector comprising the position and velocity statesof said target.
 10. A method for estimating the error in the position ofa sensor and the angular bias orientation of the sensor, said methodcomprising the steps of: operating a sensor to generate data relating toa target, said data being contaminated by sensor positional bias errorsand angular bias registration errors operating said sensor to generatesensed information relating to the target; adding to said sensed dataany sensor positional bias update information and sensor angular biasupdate information to produce updated sensed information; propagatingthe state of said updated sensed information to produce time updatedstate estimates of the target position and velocity; computing theJacobian of the state dynamics of the target; computing the statetransition matrix for the extended Kalman filter algorithm; timepropagating the covariance of a state vector comprising the position andvelocity states of said target to thereby produce positional errorinformation relating to said target.